#!/usr/bin/env python
# coding:utf-8
import cv2
import numpy as np
import math
import rospy
from ros import Ros
from rocr6_msgs.msg import Feedback
# from Camrea import Camrea
from Calibration_hand_eye import Calibration_hand_eye
from CameraClient import CameraClient, ImageType, Command, CameraIntri

def main():
    # cam = Camrea()
    camera = CameraClient()
    save_file = True
    camera_ip = "192.168.3.100"
    
    if not camera.connect(camera_ip):
        exit(-1)
    calib.File_read(cameraIntrinsicCalibrationUrl)
    # ros节点终止时，或按下Esc退出采集
    while True:
        # 从相机中获取一组帧数据，提取出color帧，转换成opencv的arr格式，最终显示在图像显示窗口中
        # color_image = cam.get_frame()
        color_image = camera.captureColorImg()
        cv2.imshow('camera image', color_image)
        kvalue = cv2.waitKey(50) & 0xFF
        if kvalue == 27:
            break
        elif kvalue == 10 or kvalue == 13:
            ret, corners = calib.findCirclesGrid(color_image)
            if ret == True:
                calib.show(color_image, ret, corners)
                calib.Cam_calibrationplate(color_image, corners)
    cv2.destroyAllWindows()
    # cam.release()  # 相机释放
    if (len(calib.target2CameraTranslationVecs) < 4):
        print('Calibration need more data !')
    else:
        if not isEyeInHand:
            Tohand_result = calib.eyeTohand(calibrationInfoUrl)
            if Tohand_result:
                print "eye_to_hand finshed"
        else:
            Inhand_result = calib.eyeInhand(calibrationInfoUrl)
            if Inhand_result:
                print "eye_in_hand finshed"

if __name__ == '__main__':
    # rospy.init_node('camera_calibrate_hand_eye')
    # isEyeInhand = bool(input('眼在手上：True，眼在手外：False : '))
    ROS = Ros()
    isEyeInHand, cameraIntrinsicCalibrationUrl, calibrationInfoUrl = ROS.ros_init()
    # cam = Camrea()
    calib = Calibration_hand_eye()
    main()
